{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2\n",
    "import pickle\n",
    "from robot import transl,r2t,t2r,rpy2tr,jtraj"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 相机建模"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 相机参数\n",
    "class Cam(object):\n",
    "    def __init__(self,name=\"0\",mtx=None,dist=None,Tr=None):\n",
    "        self.name = name\n",
    "        self.unit = 20.0 # 标定板单位为 20mm\n",
    "        self.height = 480 # 相机标定时的高\n",
    "        self.width = 640 # 相机标定时的宽\n",
    "        self.mtx = self.get_cameraMatrix() if mtx is None else mtx\n",
    "        self.dist = self.get_distCoeffs() if dist is None else dist\n",
    "        self.gen_new_image()\n",
    "        if Tr is None:\n",
    "            self.Tr = mat(eye(4))\n",
    "        else:\n",
    "            self.Tr = Tr # 世界坐标系在相机下的表示 Tc_o ,距离单位为相机的单位\n",
    "\n",
    "    def copy(self):\n",
    "        import copy\n",
    "        new = copy.copy(self)\n",
    "        new.Tr = self.Tr.copy()\n",
    "        new.mtx = self.mtx.copy()\n",
    "        new.dist = self.dist.copy()\n",
    "        new.image = self.image.copy()\n",
    "        return new\n",
    "    \n",
    "    def get_cameraMatrix(self):\n",
    "        mtx = np.eye(3)\n",
    "        fx = 1511.64442682\n",
    "        fy = 1514.72927917\n",
    "        cx = 331.982535978\n",
    "        cy = 268.613047699\n",
    "        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = fx,fy,cx,cy\n",
    "        return mtx\n",
    "\n",
    "    def get_distCoeffs(self):\n",
    "        dist = np.zeros((1,5))\n",
    "        k1 = -0.105356293294\n",
    "        k2 = 3.25067090251\n",
    "        p1 = 0.00155905212496\n",
    "        p2 = -0.00569751368371\n",
    "        k3 = -25.6347944753\n",
    "        dist[0] = k1,k2,p1,p2,k3\n",
    "        return dist\n",
    "\n",
    "    def gen_new_image(self):\n",
    "        self.image = 255+np.zeros((self.height,self.width,3),dtype=np.uint8)\n",
    "        \n",
    "    def get_image(self):\n",
    "        return self.image\n",
    "    \n",
    "    def show_image(self):\n",
    "        imshow(self.image)\n",
    "        \n",
    "    def _get_p2d_from_p3d(self,Tr,p3d):\n",
    "        p = np.vstack((p3d,1))\n",
    "        imgPoint = cv2.projectPoints(p[:3,0].reshape((1,3,1)),cv2.Rodrigues(t2r(Tr))[0],transl(Tr)[:3,0],self.mtx,self.dist)[0] # 投影到相机\n",
    "        imgPoint = imgPoint[0,0]\n",
    "        p2d = mat(imgPoint).T\n",
    "        return p2d\n",
    "    \n",
    "    def get_p2d_from_p3d(self,p3d):\n",
    "        Tr = self.Tr.copy()\n",
    "        Tr2 = Tr.copy()\n",
    "        p2d = self._get_p2d_from_p3d(Tr,p3d)\n",
    "        return p2d\n",
    "    \n",
    "    def plot_point(self,p3d,color=(255,0,0),show=True):\n",
    "        p2d = self.get_p2d_from_p3d(p3d)\n",
    "        if show:\n",
    "            self.plot_img_point(p2d,color)\n",
    "        return p2d\n",
    "        \n",
    "    def _check_point(self,x,y):\n",
    "        h,w = self.image.shape[:2]\n",
    "        return (x > 0 and x < w) and (y > 0 and y < h)\n",
    "    \n",
    "    def plot_img_point(self,p2d,color=(255,0,0)):\n",
    "        x,y = array(p2d).flatten()\n",
    "        x = int(x)\n",
    "        y = int(y)\n",
    "        if self._check_point(x,y):\n",
    "            cv2.circle(self.image, (x,y), 3, color,2)\n",
    "    \n",
    "    def plot_img_line(self,p2d0,p2d1,color=(255,0,0)):\n",
    "        a,b = array(p2d0).flatten()\n",
    "        c,d = array(p2d1).flatten()\n",
    "        a,b,c,d = int(a),int(b),int(c),int(d)\n",
    "        if self._check_point(a,b) and self._check_point(c,d):\n",
    "            cv2.line(self.image,(a,b),(c,d),color,1)\n",
    "        \n",
    "    def plot_robot_point(self,robot,q0,color=(255,0,0)):\n",
    "        p = robot.fkine(q0)[:3,3]\n",
    "        return self.plot_point(p,color)\n",
    "    \n",
    "    def get_jacobn(self,p2d,p3d,rot=True):\n",
    "        '''\n",
    "        u: 图像的x坐标\n",
    "        v: 图像的y坐标\n",
    "        z: 目标点在图像坐标系下当前的z坐标值\n",
    "        rot: 是否考虑点的空间转动,若不考虑则仅取前3列,即仅考虑点的移动\n",
    "        '''\n",
    "        u,v,z = p2d[0,0], p2d[1,0], p3d[2,0]\n",
    "        # 在图像坐标系下三维空间的图像雅克比\n",
    "        mtx = self.mtx\n",
    "        fx,fy,cx,cy = mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2]\n",
    "        if rot:\n",
    "            jacobn = np.zeros((2,6),dtype=np.float64)\n",
    "        else:\n",
    "            jacobn = np.zeros((2,3),dtype=np.float64)\n",
    "            \n",
    "        jacobn[0,0] = fx/z\n",
    "        jacobn[0,2] = -u/z\n",
    "        jacobn[1,1] = fy/z\n",
    "        jacobn[1,2] = -v/z\n",
    "\n",
    "        if rot:\n",
    "            jacobn[0,3] = -u*v/fy\n",
    "            jacobn[0,4] = (fx**2 + u**2)/fx\n",
    "            jacobn[0,5] = -fx*v/fy\n",
    "            jacobn[1,3] = -(fy**2+v**2)/fy\n",
    "            jacobn[1,4] = u*v/fx\n",
    "            jacobn[1,5] = fy*u/fx\n",
    "            \n",
    "        return mat(jacobn)\n",
    "        \n",
    "    def get_jacob0(self,p2d,p3d,rot=True):\n",
    "        '''\n",
    "        u: 图像的x坐标\n",
    "        v: 图像的y坐标\n",
    "        x: 目标点在世界坐标系下当前的x坐标值\n",
    "        y: 目标点在世界坐标系下当前的y坐标值\n",
    "        z: 目标点在世界坐标系下当前的z坐标值\n",
    "        '''\n",
    "        p = np.vstack((p3d,1))\n",
    "        cp = self.Tr*p\n",
    "        jacobn = self.get_jacobn(p2d,cp,rot)\n",
    "        R = t2r(self.Tr)\n",
    "        if rot:\n",
    "            jacob_n_o = eye(6)\n",
    "            jacob_n_o[:3,:3] = R\n",
    "            jacob_n_o[3:,3:] = R\n",
    "        else:\n",
    "            jacob_n_o = R\n",
    "        jcaob0 = jacobn*jacob_n_o\n",
    "        return jcaob0\n",
    "    \n",
    "    def get_jacobs(self,p2d,robot,q0,rot=True):\n",
    "        # 考虑全部的图像雅克比(移动和转动)\n",
    "        jacob0 = robot.jacob0(q0)\n",
    "        p3d = robot.fkine(q0)[:3,3]\n",
    "        if not rot:\n",
    "            jacob0 = jacob0[:3,:]\n",
    "        jacob_cs = self.get_jacob0(p2d,p3d,rot)\n",
    "        return jacob_cs*jacob0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def show_cams(cams):\n",
    "    for cam in cams:\n",
    "        plt.figure()\n",
    "        plt.imshow(cam.image)\n",
    "        plt.title(cam.name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def clear_cams(cams):\n",
    "    for cam in cams:\n",
    "        cam.gen_new_image()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def move_point(p3d,v3d,w3d):\n",
    "    x,y,z = array(p3d).flatten()\n",
    "    wx,wy,wz = array(w3d).flatten()\n",
    "    out_p3d = p3d.copy()\n",
    "    out_p3d += v3d\n",
    "    out_p3d[0,0] += z*wy - y*wz\n",
    "    out_p3d[1,0] += x*wz - z*wx\n",
    "    out_p3d[2,0] += y*wx - x*wy\n",
    "    return out_p3d"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 机器人的图像雅克比\n",
    "\n",
    "\n",
    "\\begin{align*}\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u} }\\\\ \n",
    "\\dot{\\mathrm{v} }\n",
    "\\end{bmatrix} &= {}^c J_s \\begin{bmatrix}\n",
    "{}^c \\dot{v}\\\\ \n",
    "{}^c \\dot{\\omega}\n",
    "\\end{bmatrix} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0 \\dot{v}\\\\ \n",
    "{}^0 \\dot{\\omega}\n",
    "\\end{bmatrix} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} J_0(q) \\dot{q} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0_n R & 0\\\\ \n",
    "0 & {}^0_n R\n",
    "\\end{bmatrix} J_n(q) \\dot{q} \\\\\n",
    "\\end{align*}\n",
    "\n",
    "\n",
    "令: \n",
    "\n",
    "$$\n",
    "J_s(q) = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0_n R & 0\\\\ \n",
    "0 & {}^0_n R\n",
    "\\end{bmatrix} J_n(q)\n",
    "$$\n",
    "\n",
    "则有:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u} }\\\\ \n",
    "\\dot{\\mathrm{v} }\n",
    "\\end{bmatrix} = J_s(q) \\dot{q}\n",
    "$$\n",
    "\n",
    "$J_s(q)$ 即机器人的图像雅克比, 若有两个图像点位于两个相机中,则有:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u}_1 }\\\\ \n",
    "\\dot{\\mathrm{v}_1 } \\\\\n",
    "\\dot{\\mathrm{u}_2 }\\\\ \n",
    "\\dot{\\mathrm{v}_2 }\n",
    "\\end{bmatrix} = \\begin{bmatrix}\n",
    "J_{s1}(q)\\\\\n",
    "J_{s2}(q)\n",
    "\\end{bmatrix} \\dot{q}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 机器人求解函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_end_point(cams,robot,q,color=(255,0,0)):\n",
    "    end_point = None\n",
    "    for cam in cams:\n",
    "        if end_point is None:\n",
    "            end_point = cam.plot_robot_point(robot,q.T,color)\n",
    "        else:\n",
    "            end_point = np.vstack((end_point,cam.plot_robot_point(robot,q.T,color)))\n",
    "    return end_point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def plot_img_points(cams,img_pts,color=(255,0,0)):\n",
    "    k = 0\n",
    "    for cam in cams:\n",
    "        cam.plot_img_point(img_pts[2*k:2*k+2,0],color)\n",
    "        k = k+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_p3d_img_point(cams,p3d,color=(255,0,0)):\n",
    "    k = 0\n",
    "    img_points = None\n",
    "    for cam in cams:\n",
    "        p2d = cam.get_p2d_from_p3d(p3d)\n",
    "        k = k+1\n",
    "        if img_points is None:\n",
    "            img_points = p2d\n",
    "        else:\n",
    "            img_points = np.vstack((img_points,p2d))\n",
    "    return img_points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def plot_end_point_line(cams,end_point0,end_point1):\n",
    "    k = 0\n",
    "    for cam in cams:\n",
    "        cam.plot_img_line(end_point0[2*k:2*k+2,0],end_point1[2*k:2*k+2,0])\n",
    "        k = k+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_jacobs_from_cams(cams,end_point,robot,q,rot=False):\n",
    "    J = None\n",
    "    k = 0\n",
    "    for cam in cams:\n",
    "        if J is None:\n",
    "            J = cam.get_jacobs(end_point[k:k+2,0],robot,q0.T,rot)\n",
    "        else:\n",
    "            J = np.vstack((J,cam.get_jacobs(end_point[k:k+2,0],robot,q0.T,rot)))\n",
    "        k = k+1\n",
    "    return J"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def be_in_good(q):\n",
    "    # 截断q, 使得q在 -pi,pi\n",
    "    for i,v in enumerate(q):\n",
    "        if v < -pi or v > pi:\n",
    "            v = v - int(v/(2*pi))*2*pi \n",
    "        if v < - pi:\n",
    "            v = v + 2*pi\n",
    "        if v > pi:\n",
    "            v= v - 2*pi\n",
    "        q[i] = v\n",
    "    return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_Jk(Jk_1,delta_q,delta_f):\n",
    "    if abs(delta_q.T*delta_q) > 0.01:\n",
    "        Jk = Jk_1+(delta_f - Jk_1*delta_q)*delta_q.T/(delta_q.T*delta_q)\n",
    "    else:\n",
    "        Jk = Jk_1\n",
    "    return Jk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_next_q(Jk,Jk_1,qk,fk,delta_q):\n",
    "    try:\n",
    "        if abs(delta_q.T*delta_q) > 0.01:\n",
    "            next_q = qk - inv(Jk.T*Jk+((Jk.T-Jk_1.T)*fk*delta_q.T)/(delta_q.T*delta_q))*(Jk.T*fk+(Jk.T-Jk_1.T)*fk)\n",
    "        else:\n",
    "            next_q = qk - inv(Jk.T*Jk)*(Jk.T*fk+(Jk.T-Jk_1.T)*fk)\n",
    "    except:\n",
    "        next_q = qk - pinv(Jk)*fk\n",
    "    return be_in_good(next_q)\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 用最小二乘估计出初始的 J0\n",
    "def get_J0(cams,robot,q0,max_delta=0.2,num=3):\n",
    "    Dp = None\n",
    "    Dq = None\n",
    "    n = len(q0) # 关节空间维度\n",
    "    m = len(cams)*2 # 图像空间维度\n",
    "    # 共有 m x n 个未知数，\n",
    "    # 每一对 dp, dq 可以提供 m 个方程\n",
    "    # 因此至少需要 n 组对应值\n",
    "    # 为实现最小二乘，现在取 2xn 组值\n",
    "    q = q0.copy()\n",
    "    p = get_end_point(cams,robot,q)\n",
    "    for i in range(n*num):\n",
    "        dq = numpy.random.random(q0.shape)*max_delta\n",
    "        dp = get_end_point(cams,robot,q+dq) - p\n",
    "        if Dp is None:\n",
    "            Dp = dp\n",
    "            Dq = dq\n",
    "        else:\n",
    "            Dp = np.hstack((Dp,dp))\n",
    "            Dq = np.hstack((Dq,dq))\n",
    "    J0 = Dp*pinv(Dq)\n",
    "    return J0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sql import Nao\n",
    "# 机器人\n",
    "nao = Nao.get_by_name(\"red\")\n",
    "robot = nao.kin.LeftHand\n",
    "q0 = mat([0.5629360675811768, -0.3141592741012573, -0.8145959377288818, -0.9541060924530029, -0.5369420051574707]).T # 机器人起始点\n",
    "p3d = robot.fkine(q0.T)[:3,3]\n",
    "Tb_c = nao.kin.BottomCamera.fkine([-0.10895586013793945, -0.018450021743774414])\n",
    "# 三个相机\n",
    "cam1 = Cam(\"cam one\")\n",
    "cam1.mtx = nao.bottom_cam.mtx\n",
    "cam1.dist = nao.bottom_cam.dist\n",
    "cam1.Tr = inv(Tb_c)\n",
    "cam2 = cam1.copy()\n",
    "cam2.name = \"cam two\"\n",
    "cam2.Tr = transl(-30,10,50)*cam2.Tr\n",
    "cam3 = cam1.copy()\n",
    "cam3.name = \"cam three\"\n",
    "cam3.Tr = transl(30,-10,60)*cam3.Tr\n",
    "#cams = [cam1,cam2]\n",
    "cams = [cam1,cam2,cam3]\n",
    "\n",
    "# 起始点\n",
    "img_point0 = get_end_point(cams,robot,q0,(0,0,0)) # 黑色\n",
    "show_cams(cams)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 生成末端点序列"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_circle(Op3d,r,num,R=np.eye(3)):\n",
    "    '''\n",
    "    画一个空间圆\n",
    "    :param Op3d: 空间圆心\n",
    "    :param r: 空间圆的直径\n",
    "    :param num: 空间圆点的数量\n",
    "    :param R: 姿态变换矩阵（默认为E，即在与 Oxy 平面平行的空间圆）\n",
    "    :return: 空间圆点序列\n",
    "    '''\n",
    "    th = np.linspace(0.0,2*pi,num)\n",
    "    x = np.sin(th)*r\n",
    "    y = np.cos(th)*r\n",
    "    z = zeros_like(x)\n",
    "    p3ds = mat(np.vstack((x,y,z)))\n",
    "    p3ds = R*p3ds\n",
    "    p3ds[0,:] += + Op3d[0,0]\n",
    "    p3ds[1,:] += + Op3d[1,0]\n",
    "    p3ds[2,:] += + Op3d[2,0]\n",
    "    return p3ds.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_img_points_list(cams,Op3d,num=100):\n",
    "    p3ds = get_circle(Op3d,30,num,t2r(rpy2tr(0.1,0.1,0.3)))\n",
    "    img_points_list = None\n",
    "    for p3d in p3ds:\n",
    "        img_points = get_p3d_img_point(cams,p3d.T)\n",
    "        plot_img_points(cams,img_points,(0,0,255)) # 绿色\n",
    "        if img_points_list is None:\n",
    "            img_points_list = img_points\n",
    "        else:\n",
    "            img_points_list = np.hstack((img_points_list,img_points))\n",
    "    return img_points_list.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams(cams)\n",
    "img_point0 = get_end_point(cams,robot,q0,(0,0,0)) # 黑色\n",
    "img_points_list = get_img_points_list(cams,p3d,50)\n",
    "show_cams(cams)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "A = np.linspace(0.0,8.0,8).reshape((2,4))\n",
    "B = np.linspace(10.0,18.0,8).reshape((2,4))\n",
    "S=[mat(A),mat(B)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.sum(S,axis=0)/2.0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for mem_index in range(0):\n",
    "    print mem_index"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 动态跟踪，next_q = qk - pinv(Jk)*(fk - dimg_point)\n",
    "def track_points_moving(cams,J0,robot,q0,img_points,n=1,delta=1.0,use_pinv=False,dimg_points=None,debug=False,memory=3):\n",
    "    # 将机器人的末端点图像移动到 img_point\n",
    "    # 初始化几个值\n",
    "    nm_list = []\n",
    "    q_list = []\n",
    "    f_list = []\n",
    "    J_list = []\n",
    "    img_point_list = []\n",
    "    end_point_list = []\n",
    "    count = [0] # 计数器，保持 count == len(q) == len(nm) == len(img_point_list) = len(end_point_list)\n",
    "    def add_to_list(qk,fk,Jk,img_point,end_point):\n",
    "        nm_list.append(fk.T*fk)\n",
    "        q_list.append(qk)\n",
    "        J_list.append(Jk)\n",
    "        f_list.append(fk)\n",
    "        img_point_list.append(img_point)\n",
    "        end_point_list.append(end_point)\n",
    "        count[0]+= 1\n",
    "        \n",
    "    qk = q0.copy()\n",
    "    Jk = J0.copy()\n",
    "    img_point = img_points[0].T\n",
    "    end_point = get_end_point(cams,robot,q0)\n",
    "    fk = end_point - img_point\n",
    "    add_to_list(qk,fk,Jk,img_point,end_point)\n",
    "    \n",
    "    while n > 0:\n",
    "        n = n - 1\n",
    "        for index,img_point in enumerate(img_points):\n",
    "            img_point = img_point.T\n",
    "            end_point = get_end_point(cams,robot,qk)\n",
    "            fk = end_point - img_point\n",
    "            qk = q_list[-1]\n",
    "            plot_end_point_line(cams,end_point,end_point_list[-1])\n",
    "                   \n",
    "            tmp_Jk_list = []\n",
    "            for mem_index in range(memory):\n",
    "                index = count[0] - 1 - mem_index\n",
    "                index = 0 if index < 0 else index\n",
    "                if index - 1 < 0:\n",
    "                    qk_1 = q_list[0]\n",
    "                else:\n",
    "                    qk_1 = q_list[index - 1]\n",
    "                fk_1 = f_list[index]\n",
    "                Jk_1 = J_list[index]\n",
    "                prv_img_point = img_point_list[index].copy()\n",
    "                # 估算雅克比\n",
    "                delta_q = qk - qk_1\n",
    "                delta_f = fk - fk_1\n",
    "                if dimg_points is None:\n",
    "                    dimg_point = img_point - prv_img_point\n",
    "                else:\n",
    "                    dimg_point = dimg_points[index].T\n",
    "\n",
    "                eps = np.finfo(np.float64).eps\n",
    "                if abs(delta_q.T*delta_q) > eps:\n",
    "                    Jk = Jk_1+(delta_f - Jk_1*delta_q + dimg_point)*delta_q.T/(delta_q.T*delta_q)\n",
    "                else:\n",
    "                    Jk = Jk_1\n",
    "                tmp_Jk_list.append(Jk)\n",
    "            # 求平均\n",
    "            Jk = np.mean(tmp_Jk_list,axis=0)\n",
    "            # 计算下一个q\n",
    "            if use_pinv:\n",
    "                dqk = - pinv(Jk)*(fk - dimg_point)\n",
    "            else:\n",
    "                try:\n",
    "                    if abs(delta_q.T*delta_q) > eps:\n",
    "                        dqk = - inv(Jk.T*Jk+((Jk.T-Jk_1.T)*fk*delta_q.T)/(delta_q.T*delta_q))*(Jk.T*fk - Jk*dimg_point -(Jk.T-Jk_1.T)*fk)\n",
    "                    else:\n",
    "                        dqk = - inv(Jk.T*Jk)*Jk.T*(fk - dimg_point)\n",
    "                except:\n",
    "                    dqk = - pinv(Jk)*(fk - dimg_point)\n",
    "            next_q = qk + dqk\n",
    "            # 保证在合理范围内\n",
    "            next_q = be_in_good(next_q)\n",
    "            qk = next_q\n",
    "            add_to_list(qk,fk,Jk,img_point,end_point)\n",
    "    return q_list,[nm_list,q_list,f_list,J_list,img_point_list,end_point_list,count]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 动态跟踪，next_q = qk - pinv(Jk)*(fk - dimg_point)\n",
    "def track_points1(cams,J0,robot,q0,img_points,n=1,use_pinv=False,dimg_points=None,debug=False):\n",
    "    # 将机器人的末端点图像移动到 img_point\n",
    "    # 初始化几个值\n",
    "    nm_list = []\n",
    "    q_list = []\n",
    "    f_list = []\n",
    "    J_list = []\n",
    "    img_point_list = []\n",
    "    end_point_list = []\n",
    "    count = [0] # 计数器，保持 count == len(q) == len(nm) == len(img_point_list) = len(end_point_list)\n",
    "    def add_to_list(qk,fk,Jk,img_point,end_point):\n",
    "        nm_list.append(fk.T*fk)\n",
    "        q_list.append(qk)\n",
    "        J_list.append(Jk)\n",
    "        f_list.append(fk)\n",
    "        img_point_list.append(img_point)\n",
    "        end_point_list.append(end_point)\n",
    "        count[0]+= 1\n",
    "        \n",
    "    qk = q0.copy()\n",
    "    Jk = J0.copy()\n",
    "    img_point = img_points[0].T\n",
    "    end_point = get_end_point(cams,robot,q0)\n",
    "    fk = end_point - img_point\n",
    "    add_to_list(qk,fk,Jk,img_point,end_point)\n",
    "    \n",
    "    while n > 0:\n",
    "        n = n - 1\n",
    "        for index,img_point in enumerate(img_points):\n",
    "            img_point = img_point.T\n",
    "            end_point = get_end_point(cams,robot,qk)\n",
    "            fk = end_point - img_point\n",
    "            prv_count = count[0] - 1\n",
    "            qk = q_list[-1].copy()\n",
    "            if count[0] > 1:\n",
    "                qk_1 = q_list[-2].copy()\n",
    "            else:\n",
    "                qk_1 = qk.copy()\n",
    "            fk_1 = f_list[-1].copy()\n",
    "            Jk_1 = J_list[-1].copy()\n",
    "            prv_img_point = img_point_list[prv_count].copy()\n",
    "            plot_end_point_line(cams,end_point,end_point_list[-1])\n",
    "                \n",
    "            # 估算雅克比\n",
    "            delta_q = qk - qk_1\n",
    "            delta_f = fk - fk_1\n",
    "            if dimg_points is None:\n",
    "                dimg_point = img_point - prv_img_point\n",
    "            else:\n",
    "                dimg_point = dimg_points[index].T\n",
    "\n",
    "            eps = np.finfo(np.float64).eps\n",
    "            if abs(delta_q.T*delta_q) > eps:\n",
    "                Jk = Jk_1+(delta_f - Jk_1*delta_q + dimg_point)*delta_q.T/(delta_q.T*delta_q)\n",
    "            else:\n",
    "                Jk = Jk_1\n",
    "\n",
    "            # 计算下一个q\n",
    "            if use_pinv:\n",
    "                dqk = - pinv(Jk)*(fk - dimg_point)\n",
    "            else:\n",
    "                try:\n",
    "                    if abs(delta_q.T*delta_q) > eps:\n",
    "                        dqk = - inv(Jk.T*Jk+((Jk.T-Jk_1.T)*fk*delta_q.T)/(delta_q.T*delta_q))*(Jk.T*fk - Jk*dimg_point -(Jk.T-Jk_1.T)*fk)\n",
    "                    else:\n",
    "                        dqk = - inv(Jk.T*Jk)*Jk.T*(fk - dimg_point)\n",
    "                except:\n",
    "                    dqk = - pinv(Jk)*(fk - dimg_point)\n",
    "            #next_q = qk + delta*dqk/norm(dqk)\n",
    "            next_q = qk + dqk\n",
    "            # 保证在合理范围内\n",
    "            next_q = be_in_good(next_q)\n",
    "            qk = next_q\n",
    "            add_to_list(qk,fk,Jk,img_point,end_point)\n",
    "    return q_list,[nm_list,q_list,f_list,J_list,img_point_list,end_point_list,count]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 静态跟踪\n",
    "def track_points_static(cams,J0,robot,q0,img_points,use_pinv=False,dimg_points=None,debug=False):\n",
    "    # 将机器人的末端点图像移动到 img_point, 两个相机\n",
    "    for index,img_point in enumerate(img_points):\n",
    "        img_point = img_point.T\n",
    "        if index == 0: # 初始化几个值\n",
    "            end_point = get_end_point(cams,robot,q0)\n",
    "            Jk_1 = J0.copy()\n",
    "            fk = end_point - img_point\n",
    "            fk_1 = fk.copy()\n",
    "            qk = q0.copy()\n",
    "            qk_1 = q0.copy()\n",
    "            nm = [fk.T*fk]\n",
    "            q = [q0]\n",
    "            prv_img_point = img_point\n",
    "            dimg_point = np.zeros_like(img_point)\n",
    "        else:\n",
    "            new_end_point = get_end_point(cams,robot,qk)\n",
    "            plot_end_point_line(cams,end_point,new_end_point)\n",
    "            end_point = new_end_point\n",
    "            fk = end_point - img_point\n",
    "            \n",
    "        # 估算雅克比\n",
    "        fk = end_point - img_point\n",
    "        delta_q = qk - qk_1\n",
    "        delta_f = fk - fk_1\n",
    "        if dimg_points is None:\n",
    "            dimg_point = img_point - prv_img_point\n",
    "        else:\n",
    "            dimg_point = dimg_points[index].T\n",
    "        \n",
    "        zero = 0.000000000001\n",
    "        if abs(delta_q.T*delta_q) > zero:\n",
    "            Jk = Jk_1+(delta_f - Jk_1*delta_q)*delta_q.T/(delta_q.T*delta_q)\n",
    "        else:\n",
    "            Jk = Jk_1\n",
    "        \n",
    "        # 计算下一个q\n",
    "        if use_pinv:\n",
    "            next_q = qk - pinv(Jk)*(fk - dimg_point)\n",
    "        else:\n",
    "            try:\n",
    "                if abs(delta_q.T*delta_q) > zero:\n",
    "                    next_q = qk - inv(Jk.T*Jk+((Jk.T-Jk_1.T)*fk*delta_q.T)/(delta_q.T*delta_q))*(Jk.T*fk -(Jk.T-Jk_1.T)*fk)\n",
    "                else:\n",
    "                    next_q = qk - inv(Jk.T*Jk)*Jk.T*fk\n",
    "            except:\n",
    "                next_q = qk - pinv(Jk)*fk\n",
    "                \n",
    "        # 保证在合理范围内\n",
    "        next_q = be_in_good(next_q)\n",
    "        \n",
    "        # 下一次迭代准备\n",
    "        fk_1 = fk # fk 的计算在开头,因为每一次迭代 img_point 都不同\n",
    "        qk_1 = qk\n",
    "        qk = next_q\n",
    "        \n",
    "        if debug:\n",
    "            print count, fk\n",
    "            if nm < fk.T*fk:\n",
    "                print \"发散\"\n",
    "            else:\n",
    "                print \"收敛\"\n",
    "        q.append(qk)\n",
    "        nm.append(fk.T*fk)\n",
    "    return q,[nm_list,q_list,f_list,J_list,img_point_list,end_point_list,count]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams(cams)\n",
    "J0 = get_J0(cams,robot,q0,max_delta=0.1,num=10)\n",
    "show_cams(cams)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "num = 100 # 每一圈的分割数，越多越慢\n",
    "n = 5 # 转的圈数\n",
    "cams = [cam1,cam2]\n",
    "#cams = [cam1,cam2,cam3]\n",
    "J0 = get_J0(cams,robot,q0,max_delta=0.01,num=100)\n",
    "clear_cams(cams)\n",
    "img_points_list = get_img_points_list(cams,p3d,num)\n",
    "q,all_list = track_points1(cams,J0,robot,q0,img_points_list,n=n,use_pinv=False)\n",
    "#print array(nm).flatten().tolist()\n",
    "show_cams(cams)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "num = 100 # 每一圈的分割数，越多越慢\n",
    "n = 3 # 转的圈数\n",
    "cams = [cam1,cam2]\n",
    "J0 = get_J0(cams,robot,q0,max_delta=0.1,num=10)\n",
    "clear_cams(cams)\n",
    "img_points_list = get_img_points_list(cams,p3d,num)\n",
    "q = track_points_moving(cams,J0,robot,q0,img_points_list,n=n,use_pinv=False,memory=1)\n",
    "#print array(nm).flatten().tolist()\n",
    "show_cams(cams)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "_tmp_cams = [cam1.copy(),cam2.copy()]\n",
    "for cam in _tmp_cams:\n",
    "    cam.name += \" num= %d delta=%.3f\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def plot_imgs(num=100,delta=0.01):\n",
    "    num = int(num)\n",
    "    #for delta in 0.01+0.01*1.618*np.arange(3):  # 机器臂单步变换量，越大越快\n",
    "    n = 3 # 转的圈数\n",
    "    tmp_cams = [cam.copy() for cam in _tmp_cams]\n",
    "    for cam in tmp_cams:\n",
    "        cam.name = cam.name % (num,delta)\n",
    "    clear_cams(tmp_cams)\n",
    "    img_points_list = get_img_points_list(tmp_cams,p3d,num)\n",
    "    q = track_points1(tmp_cams,J0,robot,q0,img_points_list,n=n,delta=delta,use_pinv=False)\n",
    "    #print array(nm).flatten().tolist()\n",
    "    show_cams([tmp_cams[0]])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
